#include "stdafx.h"
#include "PhysicsPrimitives.h"
#include "Ray.h"
#include "PhysicsSystem.h"

static XMFLOAT3X3 calculateBoxInertiaTensor(float sizeX, float sizeY, float sizeZ, float mass)
{
	float iX = mass * (sizeY * sizeY + sizeZ * sizeZ ) / 12.0f * 1;
	float iY = mass * (sizeX * sizeX + sizeZ * sizeZ ) / 12.0f * 1;
	float iZ = mass * (sizeX * sizeX + sizeY * sizeY ) / 12.0f * 1;

	return XMFLOAT3X3(	iX, 0.0f, 0.0f,
						0.0f, iY, 0.0f,
						0.0f, 0.0f, iZ);
}

void mphys::Box::reset(const RigidBodyDesc & desc)
{
	HalfSizeX = 17.0f * desc.Scale.x;
	HalfSizeY = 17.0f * desc.Scale.y;
	HalfSizeZ = 17.0f * desc.Scale.z;

	if(desc.IsStatic)
	{
		initStatic(desc.Position, desc.Orientation);
	}
	else
	{
		assert(desc.Density > 0.0f);

		float mass = 8 * HalfSizeX * HalfSizeY * HalfSizeZ * desc.Density;
		XMFLOAT3X3 inertiaTensor = calculateBoxInertiaTensor(2 * HalfSizeX, 2 * HalfSizeY, 2 * HalfSizeZ, mass);

		initDynamic(desc.Position, desc.Orientation, mass, inertiaTensor);
	}

	updateBoxData();
}

void mphys::Box::updateBoxData()
{
	const XMVECTOR x = XMVectorSet(1, 0, 0, 0);
	const XMVECTOR y = XMVectorSet(0, 1, 0, 0);
	const XMVECTOR z = XMVectorSet(0, 0, 1, 0);

	XMMATRIX worldMatrix = XMLoadFloat4x4( &WorldMatrix );

	XMVECTOR xAxis = XMVector3TransformNormal(x, worldMatrix);
	XMVECTOR yAxis = XMVector3TransformNormal(y, worldMatrix);
	XMVECTOR zAxis = XMVector3TransformNormal(z, worldMatrix);

	XMStoreFloat3( &Axes[0], xAxis );
	XMStoreFloat3( &Axes[1], yAxis );
	XMStoreFloat3( &Axes[2], zAxis );
	
	XMVECTOR position = XMLoadFloat3(&Position);
	XMVECTOR pointOnFace;

	pointOnFace = XMVectorAdd(position, XMVectorMultiply(xAxis, XMVectorReplicate(HalfSizeX)));
	mFaceOffsetFromOrigin[0] = XMVectorGetX(XMVector3Dot(pointOnFace, xAxis));

	xAxis = XMVectorNegate(xAxis);
	pointOnFace = XMVectorAdd(position, XMVectorMultiply(xAxis, XMVectorReplicate(HalfSizeX)));
	mFaceOffsetFromOrigin[1] = XMVectorGetX(XMVector3Dot(pointOnFace, xAxis));


	pointOnFace = XMVectorAdd(position, XMVectorMultiply(yAxis, XMVectorReplicate(HalfSizeY)));
	mFaceOffsetFromOrigin[2] = XMVectorGetX(XMVector3Dot(pointOnFace, yAxis));

	yAxis = XMVectorNegate(yAxis);
	pointOnFace = XMVectorAdd(position, XMVectorMultiply(yAxis, XMVectorReplicate(HalfSizeY)));
	mFaceOffsetFromOrigin[3] = XMVectorGetX(XMVector3Dot(pointOnFace, yAxis));


	pointOnFace = XMVectorAdd(position, XMVectorMultiply(zAxis, XMVectorReplicate(HalfSizeZ)));
	mFaceOffsetFromOrigin[4] = XMVectorGetX(XMVector3Dot(pointOnFace, zAxis));

	zAxis = XMVectorNegate(zAxis);
	pointOnFace = XMVectorAdd(position, XMVectorMultiply(zAxis, XMVectorReplicate(HalfSizeZ)));
	mFaceOffsetFromOrigin[5] = XMVectorGetX(XMVector3Dot(pointOnFace, zAxis));
}

float mphys::Box::checkRayIntersection( const mmath::Ray & ray )
{
	XMVECTOR rayOrig = XMLoadFloat3(&ray.Origin);
	XMVECTOR rayDir = XMLoadFloat3(&ray.Direction);

	float plDep0, plDep1;
	float tNear = -std::numeric_limits<float>::infinity(), tFar = std::numeric_limits<float>::infinity();
	float origDotNorm, oneOverNormDotDir;

	XMVECTOR xNormal = XMLoadFloat3(&Axes[0]);
	origDotNorm = XMVectorGetX(XMVector3Dot(rayOrig, xNormal));
	oneOverNormDotDir = 1.0f / XMVectorGetX(XMVector3Dot(xNormal, rayDir));
	plDep0 = (mFaceOffsetFromOrigin[0] - origDotNorm) * oneOverNormDotDir;
	plDep1 = (mFaceOffsetFromOrigin[1] + origDotNorm) * -oneOverNormDotDir;

	if(plDep0 > plDep1) { tFar = plDep0; tNear = plDep1; }
	else			    { tFar = plDep1; tNear = plDep0; }

	XMVECTOR yNormal = XMLoadFloat3(&Axes[1]);
	origDotNorm = XMVectorGetX(XMVector3Dot(rayOrig, yNormal));
	oneOverNormDotDir = 1.0f / XMVectorGetX(XMVector3Dot(yNormal, rayDir));
	plDep0 = (mFaceOffsetFromOrigin[2] - origDotNorm) * oneOverNormDotDir;
	plDep1 = (mFaceOffsetFromOrigin[3] + origDotNorm) * -oneOverNormDotDir;

	if(plDep0 > plDep1) { tFar = (std::min)(tFar, plDep0); tNear = (std::max)(tNear, plDep1); }
	else				{ tFar = (std::min)(tFar, plDep1); tNear = (std::max)(tNear, plDep0); }

	XMVECTOR zNormal = XMLoadFloat3(&Axes[2]);
	origDotNorm = XMVectorGetX(XMVector3Dot(rayOrig, zNormal));
	oneOverNormDotDir = 1.0f / XMVectorGetX(XMVector3Dot(zNormal, rayDir));
	plDep0 = (mFaceOffsetFromOrigin[4] - origDotNorm) * oneOverNormDotDir;
	plDep1 = (mFaceOffsetFromOrigin[5] + origDotNorm) * -oneOverNormDotDir;

	if(plDep0 > plDep1) { tFar = (std::min)(tFar, plDep0); tNear = (std::max)(tNear, plDep1); }
	else				{ tFar = (std::min)(tFar, plDep1); tNear = (std::max)(tNear, plDep0); }

	if(tFar < tNear || tNear < 0)
		return std::numeric_limits<float>::infinity();

	return tNear;
}

mphys::RigidBody & mphys::Box::copy()
{
	return System->createBoxCopy(*this);
}

static XMFLOAT3X3 calculateSphereInertiaTensor(float radius, float mass)
{
	float i = 2.0f / 5.0f * mass * radius * radius;

	return XMFLOAT3X3(	i, 0.0f, 0.0f,
						0.0f, i, 0.0f,
						0.0f, 0.0f, i);
}

void mphys::Sphere::reset( const RigidBodyDesc & desc )
{
	Radius = 33.41f * (std::max)(desc.Scale.x, (std::max)(desc.Scale.y, desc.Scale.z));

	if(desc.IsStatic)
	{
		initStatic(desc.Position, desc.Orientation);
	}
	else
	{
		assert(desc.Density > 0.0f);

		float mass = 4.0f / 3.0f * PI * Radius * Radius * Radius * desc.Density;
		XMFLOAT3X3 inertiaTensor = calculateSphereInertiaTensor(Radius, mass);

		initDynamic(desc.Position, desc.Orientation, mass, inertiaTensor);
	}	
}

float mphys::Sphere::checkRayIntersection( const mmath::Ray & ray )
{
	return ray.intersectSphere(XMLoadFloat3(&Position), Radius);
}

mphys::RigidBody & mphys::Sphere::copy()
{
	return System->createSphereCopy(*this);
}

void mphys::Plane::reset( const RigidBodyDesc & desc )
{
	initStatic( desc.Position, desc.Orientation );
	updatePlaneData();
}

void mphys::Plane::updatePlaneData()
{
	XMMATRIX matrix = XMMatrixRotationQuaternion( XMLoadFloat4( &Orientation ) );
	XMVECTOR normal = XMVector3TransformNormal( XMVectorSet( 0.0f, 0.0f, 1.0f, 0.0f ), matrix );
	XMStoreFloat3( &Normal, normal );
	Offset = XMVectorGetX( XMVector3Dot( normal, XMLoadFloat3( &Position ) ) );
}

float mphys::Plane::checkRayIntersection( const mmath::Ray & ray )
{
	XMVECTOR normal = XMLoadFloat3(&Normal);
	float x = XMVectorGetX(XMVector3Dot(XMLoadFloat3(&ray.Origin), normal));
	float y = XMVectorGetX(XMVector3Dot(normal, XMLoadFloat3(&ray.Direction)));
	return  (Offset - x) / y;
}

mphys::RigidBody & mphys::Plane::copy()
{
	return System->createPlaneCopy(*this);
}
